/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "MaCI_BehaviourFrame.h"




DEFINE_EVENT_TYPE(MY_EVENT_REFRESHSPEEDVALUES);
DEFINE_EVENT_TYPE(MY_EVENT_REFRESHBEHAVIOURCOLORS);

//cretae event table for catching custom event
BEGIN_EVENT_TABLE(MaCI_BehaviourFrame, wxFrame)
  EVT_COMMAND(wxID_ANY, MY_EVENT_REFRESHSPEEDVALUES, MaCI_BehaviourFrame::OnNewSpeedValues)
  EVT_COMMAND(wxID_ANY, MY_EVENT_REFRESHBEHAVIOURCOLORS, MaCI_BehaviourFrame::OnNewBehaviourState)
END_EVENT_TABLE();

MaCI_BehaviourFrame::MaCI_BehaviourFrame( wxWindow* parent )
  : CSync(1,1),
    CThread(1),
    Frame( parent ),
    iBControllingDataArray(),
    iBControllingValueTime(),
    iBehaviourControlling(),
    iBehaviourStates(),
    iServiceList(),
    iServiceListUpdated(false),
    iGIMIPtr(NULL),
    iSelectedBehaviour(-1),
    iBehaviourClientArray(),
    iThreadRunning(false)
{
  m_listCtrl_behaviours->InsertColumn(0, wxT("Behaviours"));
  m_listCtrl_behaviours->SetColumnWidth(0, 600);

  m_listCtrl_behaviours->InsertColumn(1, wxT("Speed"));
  m_listCtrl_behaviours->SetColumnWidth(1, 100);

  m_listCtrl_behaviours->InsertColumn(2, wxT("Angular speed/angle"));
  m_listCtrl_behaviours->SetColumnWidth(2, 200);
}
//*****************************************************************************
MaCI_BehaviourFrame::~MaCI_BehaviourFrame()
{
  Deinitialize();
}
//*****************************************************************************

void MaCI_BehaviourFrame::OnItemActivate( wxListEvent& event )
{
  
  MaCI::Behaviour::CBehaviourClient *bc =  (MaCI::Behaviour::CBehaviourClient *)(m_listCtrl_behaviours->GetItemData(event.GetIndex()));
  OnStartStopButton(event);	
}
//*****************************************************************************

void MaCI_BehaviourFrame::OnItemSelected( wxListEvent& event )
{
  dPrint(10,"Got event, selected %d , event %d", iSelectedBehaviour, event.GetIndex());
  if(iSelectedBehaviour != event.GetIndex()){
    if(iSelectedBehaviour != -1){
      dPrint(10,"Setting white color, %d", iSelectedBehaviour);
      wxColour selectColour(wxString("WHITE"));
      m_listCtrl_behaviours->SetItemBackgroundColour(iSelectedBehaviour,selectColour);
      CSync::Lock();
      iBehaviourControlling[iSelectedBehaviour] = false;
      CSync::Unlock();
    }

   
    CSync::Lock();
    iSelectedBehaviour = event.GetIndex();
    CSync::Unlock();
    //Deselect the item to get own colour to selected item
    m_listCtrl_behaviours->SetItemState(event.GetIndex(), 0, wxLIST_STATE_SELECTED);
    dPrint(10,"Setting grey color, %d", event.GetIndex());
    wxColour selectColour(wxString("LIGHT GREY"));
    m_listCtrl_behaviours->SetItemBackgroundColour(event.GetIndex(),selectColour);
    wxCommandEvent e( MY_EVENT_REFRESHSPEEDVALUES, GetId() );
    AddPendingEvent(e);
  }else{
    
    m_listCtrl_behaviours->SetItemState(event.GetIndex(), 0, wxLIST_STATE_SELECTED);
  }
 
  dPrint(10,"Selected %d", iSelectedBehaviour);
}
//*****************************************************************************

void MaCI_BehaviourFrame::OnStartStopButton( wxCommandEvent& event )
{
  if(iSelectedBehaviour >= 0 && iSelectedBehaviour < iBehaviourClientArray.size()){
    if(iBehaviourClientArray[iSelectedBehaviour]->GetBehaviourState()){
      iBehaviourClientArray[iSelectedBehaviour]->SetStop();
    }else{
      iBehaviourClientArray[iSelectedBehaviour]->SetStart();
    }
  }
}
//*****************************************************************************

bool MaCI_BehaviourFrame:: Initialize(gimi::GIMI *aGimiPtr, int aGimboID, int aReportID)
{
  bool result = true;
  dPrint(10,"Initializing");
  // Store for future reference.
  iGIMIPtr = aGimiPtr;
  
  // Start the Gimbo core.
  CComponent::Start(aGimiPtr, aGimboID, aReportID);
  
  // Unconditional LoadConfig.
  //  LoadConfig();

  iThreadRunning = true;
  CThread::RunThread();
  
  wxCommandEvent e( MY_EVENT_REFRESHBEHAVIOURCOLORS, GetId() );
  AddPendingEvent(e);
  return result;

}
//*****************************************************************************

void MaCI_BehaviourFrame::Deinitialize(void)
{
  iThreadRunning = false;
 DisconnectServices();

}
//*****************************************************************************

void MaCI_BehaviourFrame::DisconnectServices(void)
{
  dPrint(ODTEST,"DisconnectServices() (%u entries in array)", iBehaviourClientArray.size());
  // Call Close() on all Clients and then delete.
  for(EACH_IN_i(iBehaviourClientArray)) {
    (*i)->Close();
    delete *i;
  }
  
  // Clear the array.
  iBehaviourClientArray.clear();
}
//*****************************************************************************

void MaCI_BehaviourFrame::OnServiceAssignment(const Gimbo::TServiceAssignmentList &aServiceList)
{
 
  dPrint(10,"Event: OnServiceAssignment");
  
  // Got service list - REPLACING current.
  iServiceList = aServiceList;
  iServiceListUpdated = true;
  
  // Disconnect services (if any)
  DisconnectServices();

  dPrint(10,"Disconnect - Done");

  // Connect services.
  ConnectServices();

  // RequestUpdateStatusBar();

  dPrint(10,"Connect - Done");
}
//*****************************************************************************

bool MaCI_BehaviourFrame::ConnectServices(void) 
{
  bool result = iServiceList.size() > 0;
  dPrint(ODTEST,"ConnectServices()");

  assert(iGIMIPtr != NULL);
  int behNumber =  0;
  for(EACH_IN_i(iServiceList)) {
    // Create new instance
    MaCI::Behaviour::CBehaviourClient *bc = new MaCI::Behaviour::CBehaviourClient(iGIMIPtr, i->minorType);
    assert(bc != NULL);

    dPrint(10,"Constructed new BehaviourClient (Minor: %d)", i->minorType);
 
    // Assign
    dPrint(10,"Setting Default target : 0x%08x", i->targetID);
    bc->SetDefaultTarget(i->targetID);
    
    // Open!
    dPrint(10,"Calling Open()");
    MaCI::EMaCIError e = bc->Open();
    
    //Then get MaCI name
    if (e == MaCI::KMaCIOK) {
      dPrint(ODTEST,"Successfully opened BehaviourClient to target: '%u', using minor %u.", 
             i->targetID, i->minorType);
      

      const MaCI::MaCICtrl::SMaCISL sl = bc->GetDefaultTargetMaCISL();
      wxString behaviourName;

      MaCI::MaCICtrl::CMaCICtrlClient MaCICtrl(GetGimi());
      
      if (MaCICtrl.Open()!=MaCI::KMaCIOK){
        
        dPrint(5,"Couldn't open MaCICtrl client");
      }
      
      //Get names
      MaCI::MaCICtrl::TServiceEntryArray services;
      MaCI::MaCICtrl::TServiceEntryArray::iterator iter;
      int counter = 0;
      bool found = false;
      while (!found && counter<=5){
        MaCICtrl.GetServiceArray(services, bc->GetDefaultTarget(),counter,1000);
        counter++;
        //for (iter=services.begin();iter!=services.end();iter++)
          if (services.size())
            {
              for (iter=services.begin();iter!=services.end();iter++)
                {
                  if (iter->service.servicemajor==i->majorType && 
                      iter->service.serviceminor==i->minorType)
                    {
                      found=true;
                      break;
                    }
                }
            }
         
      }

      if(found){
        behaviourName = iter->GetMaCISL().ToMaCISLString().c_str();
        dPrint(10,"Name is %s", iter->GetMaCISL().ToMaCISLString().c_str());
      } else {
        dPrint(10,"Not valid");
        behaviourName = bc->GetDefaultTarget();
        
      } 
      CSync::Lock();
      iBehaviourStates.push_back(bc->GetBehaviourState());
      MaCI::Behaviour::CBehaviourData bdata;
      iBControllingDataArray.push_back(bdata);
      gim::time time; 
      iBControllingValueTime.push_back(time);
      iBehaviourControlling.push_back(false);
      //Insert item
      int index =m_listCtrl_behaviours->InsertItem(behNumber, behaviourName);
      behNumber++;
      m_listCtrl_behaviours->SetItemData(index, (wxUIntPtr)&(*bc) );
      // Register handler!
      bc->SetBehaviourClientCallback(this);
      iBehaviourClientArray.push_back(bc);
      CSync::Unlock();

      

    } else {
      dPrint(10,"Failed to Connect to BehaviourServer at target: '%u', using minor %u!",
             i->targetID, i->minorType);
    }
  }

  return result;
}
//*****************************************************************************

void  MaCI_BehaviourFrame::OnBehaviourCtrlEvent(const MaCI::Behaviour::CBehaviourData &aData, 
                                                const unsigned int aDataSequence,
                                                MaCI::Behaviour::CBehaviourClient &aBehaviourClient)
{
  
  for(int i = 0; i< iBehaviourClientArray.size();i++){
    if(iBehaviourClientArray[i] == &aBehaviourClient){
      CSync::Lock();
      iBControllingDataArray[i] = aData;
      gim::time time;
      time.setToCurrent();
      iBControllingValueTime[i] = time;
      iBehaviourControlling[i] = true;
      CSync::Unlock();
      wxCommandEvent e( MY_EVENT_REFRESHSPEEDVALUES, GetId() );
      AddPendingEvent(e);

    }
  }

}
//*****************************************************************************

void MaCI_BehaviourFrame::OnBehaviourStateEvent(const MaCI::Behaviour::CBehaviourData &aData, 
                                                const unsigned int aDataSequence,
                                                MaCI::Behaviour::CBehaviourClient &aBehaviourClient)
{

  //Behaviour is off
  for(int i = 0; i < iBehaviourClientArray.size(); i++){
    
    if(iBehaviourClientArray[i] == &aBehaviourClient){
      CSync::Lock();
      if(aData.GetBehaviourState()->state == 0){
        iBehaviourStates[i] = false;
      }else{
        iBehaviourStates[i] = true;
      }
      CSync::Unlock();
    }
  }
  
  
  wxCommandEvent e( MY_EVENT_REFRESHBEHAVIOURCOLORS, GetId() );
  AddPendingEvent(e);
    
}
//*****************************************************************************

void MaCI_BehaviourFrame::OnNewSpeedValues(wxCommandEvent &aEvent)
{
  for(int i = 0; i< iBehaviourClientArray.size(); i++){
    if(iBehaviourControlling[i]){
      const MaCI::Behaviour::TBehaviourControl *bc = iBControllingDataArray[i].GetBehaviourControl();
      if(bc){
        wxString speedValue;
        speedValue.Printf(wxT("%f"), bc->speed);
        m_listCtrl_behaviours->SetItem(i,1,speedValue);
        
        wxString angleValue;
        angleValue.Printf(wxT("%f"), bc->angle);
        wxString rad(" rad");
         m_listCtrl_behaviours->SetItem(i,2,angleValue+rad);
      }else{
        const MaCI::Behaviour::TBehaviourControlSpeed *bcs = iBControllingDataArray[i].GetBehaviourControlSpeed();
        if(bcs){
          wxString speedValue;
          speedValue.Printf(wxT("%f"), bcs->speed);
           m_listCtrl_behaviours->SetItem(i,1,speedValue);
          
          wxString angleValue;
          angleValue.Printf(wxT("%f"), bcs->angularSpeed);
          wxString rad(" rad/s");
           m_listCtrl_behaviours->SetItem(i,2,angleValue+rad);
          
        }
      }

    }else{
      wxString speedValue("-");
       m_listCtrl_behaviours->SetItem(i,1,speedValue);
      
      wxString angleValue("-");
       m_listCtrl_behaviours->SetItem(i,2,angleValue);
      
    }

  }
  
}
//*****************************************************************************

void  MaCI_BehaviourFrame::OnNewBehaviourState(wxCommandEvent &aEvent)
{

  for (int i = 0; i <iBehaviourStates.size();i++){
    if(iBehaviourStates[i]){
      wxColour green(0,255,0);
      m_listCtrl_behaviours->SetItemTextColour(i,green);
      
    }else{
      wxColour red(255,0,0);
      m_listCtrl_behaviours->SetItemTextColour(i,red);
    }
  }
  
}
//*****************************************************************************

int MaCI_BehaviourFrame::ThreadFunction(const int aThreadNumber)
{
    
  dPrint(4, "Update Thread started");
  
  // Run as long as the thread should be active
  while(iThreadRunning) {
    // Call the ReceiveMessage wrapper with 1s timeout
    
    gim::time timenow;
    timenow.setToCurrent();
    bool update = false;
    for(int i = 0; i< iBControllingDataArray.size(); i++){
      if(iBehaviourControlling[i]){
        if(timenow -iBControllingValueTime[i] > 500){
          CSync::Lock();
          iBehaviourControlling[i] = false;
          update = true;
          CSync::Unlock();
        }
      }
    }
    if(update){
      wxCommandEvent e( MY_EVENT_REFRESHSPEEDVALUES, GetId() );
      AddPendingEvent(e);
    }
    

    ownSleep_ms(500);
  }
  
  dPrint(4, "Update Thread stopped");
 
  return 0;

}
